#include "run.h"
#include "main.h"
#include "motor.h"
#include "MG90.h"
#include "key.h"

uint8_t run_Mode = 0;         // 当前运行模式
uint8_t run_Mode_last;        // 上一次运行模式
uint8_t auto_mode;            // 自动模式的子模式
uint8_t auto_mode1;           // 半自动模式的子模式
uint8_t ball_Flag;            // 小球检测标志位
uint8_t pos;
extern int16_t PI_data[2];    // 0 - 左轮速度, 1 - 右轮速度

void run_Proc(void)
{
    switch(run_Mode)
    {
        case 0: // 初始状态以及等待用户选择模式
            break;

        case 1: // 自动模式
            switch(auto_mode)
            {
                case 0: // 初始化第一个子模式
                    motor_set(0, 0, 0, 0); // 停止电机
                    auto_mode = 1;         // 进入下一子模式
                    break;

                case 1: // 寻找小球模式
                    motor_set(20, -80, 20, -80); // 向右旋转搜索
                    if(ball_Flag == 1) auto_mode = 2; // 检测到小球进入跟踪模式
                    break;

                case 2: // 跟踪小球模式
                    motor_set(PI_data[0], PI_data[1], PI_data[0], PI_data[1]); // 根据PID控制运动
                    if(ball_Flag == 0) auto_mode = 1; // 小球丢失返回寻找模式
                    else if(PI_data[0] == 0 && PI_data[1] == 0 && pos == 2) // 到达抓取位置
                    {
                        run_Mode_last = run_Mode; // 记录当前运行模式
                        run_Mode = 3;             // 切换为抓取模式
                    }
                    break;
            }
            break;

        case 2: // 半自动模式
            switch(auto_mode1)
            {
                case 0: // 初始化第一个子模式
                    motor_set(0, 0, 0, 0); // 停止电机
                    auto_mode1 = 1;        // 进入手动控制模式
                    break;

                case 1: // 手动控制模式
                    if(ball_Flag == 1) auto_mode1 = 2; // 检测到小球进入跟踪模式
                    break;

                case 2: // 跟踪小球模式
                    motor_set(PI_data[0], PI_data[1], PI_data[0], PI_data[1]); // 跟踪小球
                    if(ball_Flag == 0)
                    {
                        motor_set(0, 0, 0, 0); // 小球丢失停止电机
                        auto_mode1 = 1;       // 返回手动控制模式
                    }
                    else if(PI_data[0] == 0 && PI_data[1] == 0 && pos == 2) // 准备抓取
                    {
                        run_Mode_last = run_Mode; // 记录当前运行模式
                        run_Mode = 3;             // 切换为抓取模式
                    }
                    break;
            }
            break;

        case 3: // 抓取模式
            motor_set(0, 0, 0, 0); // 停止所有电机
            if(PI_data[0] == 0 && PI_data[1] == 0) // 确保已经停止移动
            {
                zhuaqu();   // 执行抓取动作
                huizhong(); // 回中位
            }
            else
            {
                run_Mode = run_Mode_last; // 如果未到位则恢复上一模式
            }
            run_Mode = run_Mode_last; // 抓取完成后恢复上一模式
            break;

        case 4: // 放球模式
            zhuaqu();     // 执行放球动作（可能是反向抓取）
            run_Mode = 5; // 切换到完成状态
            break;
    }
}
